package gwt.seca.client.agents;

import static gwt.g3d.client.math.MatrixStack.MODELVIEW;

import javax.vecmath.Vector3f;
import javax.vecmath.Vector4f;

import ernest84d.Ernest;
import ernest84d.Visual82SensorymotorSystem;
import gwt.g2d.client.graphics.Color;
import gwt.seca.client.Block;
import gwt.seca.client.Renderer;
import gwt.seca.client.Resources;
import gwt.seca.client.SceneManager;
import gwt.seca.client.util.Pair;
import gwt.seca.client.util.SecaColor;
import gwt.seca.client.util.SecaMath;
import gwt.seca.client.util.Sphere;

public class Ernest8Model extends AbstractModel{
	
	public enum Version {v8_0, v8_1, v8_2, v8_3, v8_4};
	
	//Animatable parts
	public Vector3f mCephalothoraxTrans = new Vector3f();
	public Vector3f mCephalothoraxRot = new Vector3f();
	public Vector3f mFrontLegsRot = new Vector3f();
	public Vector3f mLeftLegsRot = new Vector3f();
	public Vector3f mRightLegsRot = new Vector3f();
	public Vector3f mBackLegsRot = new Vector3f();
	public Vector3f mTailRot = new Vector3f();
	
	public Ernest8Model(SceneManager sceneMgr, String name) {
		super(sceneMgr,name);
		mAnimation = new Ernest8Anim(this);
		mDiscrete = false;
		mLocalBoundingSphere = new Sphere(new Vector3f(), .2f);
	}
	
	/**
	 * Initialize the Ernest agent.
	 */
	public void init() {//v8_4
		
		mErnest = new Ernest();
		mSensorymotorSystem = new Visual82SensorymotorSystem();
//		mTracer = new Tracer("trace.txt");
		
		// Initialize the Ernest === 
		mErnest.setParameters(6, 1, 4);
		mErnest.setSensorymotorSystem(mSensorymotorSystem);
//		mErnest.setTracer(mTracer);
		// Ernest's inborn primitive interactions
		mSensorymotorSystem.addPrimitiveAct(">", true,  -10); // Move
		mSensorymotorSystem.addPrimitiveAct(">", false, -80); // Bump
		mSensorymotorSystem.addPrimitiveAct("^", true,    0); // Left toward empty
		mSensorymotorSystem.addPrimitiveAct("^", false, -50); // Left toward wall
		mSensorymotorSystem.addPrimitiveAct("v", true,    0); // Right toward empty
		mSensorymotorSystem.addPrimitiveAct("v", false, -50); // Right toward wall
//		System.out.println("Ernest initialized") ;
		
		mIntention = "";
		mStatus = false;
		mEyeAngle = 45;
		mPixelLeft = new Color(0, 0, 0);
		mPixelRight = new Color(0, 0, 0);
		mVisualMatrix = new int[2][1];
		mVisualMatrix[0][0] = mErnest.INFINITE;
		mVisualMatrix[1][0] = mErnest.INFINITE;
	}

	/**
	 * Close the Ernest agent.
	 */
	public void dispose() {
		//TODO
//		mTracer.close();
//		mOrientation = ORIENTATION_UP;
		mErnest = null;
	}
	/**
	 * Update the agent. When the parameter 'elaspsedTime' is not used, this method is equivalent to a 'tick' method.
	 */
	protected void updateAgent(double elaspsedTime) {
		mIntention = stepErnest(mStatus);
		mStatus = enactSchema(mIntention);
	}
	
	protected void drawAgent(Renderer renderer) {
		renderer.enableUniformColor();
		
		//Transformations T Rzyx S
		//Ernest is a horseshoe crab
		//1 cephalothorax (1 hemisphere)
		//	2 eyes
		//	12 legs
		//	1 abdomen (1 hemisphere)
		//		1 tail (1 box)
		{ //Cephalothorax
			renderer.setColorUniform(new Vector4f(0.8f, 0f, 0f, 1f));
			MODELVIEW.push();
			MODELVIEW.translate(mCephalothoraxTrans.x, mCephalothoraxTrans.y, mCephalothoraxTrans.z-.45f);
			MODELVIEW.rotateZ((float) Math.toRadians(mCephalothoraxRot.z));
			MODELVIEW.rotateY((float) Math.toRadians(mCephalothoraxRot.y));
			MODELVIEW.rotateX((float) Math.toRadians(mCephalothoraxRot.x));
			MODELVIEW.scale(0.3f/2, 0.3f/2, 0.2f/2);
			renderer.setMatrixUniforms();
			Resources.mMeshList[Resources.MESH_ID_HEMISPHERE].draw();
			{ //Eyes (1 eye = 1 box)
				//Left eye
				renderer.setColorUniform(new Vector4f(mPixelLeft.getR()/255f, mPixelLeft.getG()/255f, mPixelLeft.getB()/255f, 1f));
				MODELVIEW.push();
				MODELVIEW.translate(.7f, .4f, .5f);
				MODELVIEW.scale(0.2f, 0.2f, 0.3f);
				renderer.setMatrixUniforms();
				Resources.mMeshList[Resources.MESH_ID_BOX].draw();
				MODELVIEW.pop();
				//Right eye
				renderer.setColorUniform(new Vector4f(mPixelRight.getR()/255f, mPixelRight.getG()/255f, mPixelRight.getB()/255f, 1f));
				MODELVIEW.push();
				MODELVIEW.translate(.7f, -.4f, .5f);
				MODELVIEW.scale(0.2f, 0.2f, 0.3f);
				renderer.setMatrixUniforms();
				Resources.mMeshList[Resources.MESH_ID_BOX].draw();
				MODELVIEW.pop();
			}
			{ //12 legs (1 leg = 1 box)
				renderer.setColorUniform(new Vector4f(0.8f, 0f, 0f, 1f));
				//2 front legs
				MODELVIEW.push();
				MODELVIEW.translate(.6f, 0, 0);
				{
					//Left leg
					MODELVIEW.push();
					MODELVIEW.translate(0, .2f, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mFrontLegsRot.z));
					MODELVIEW.rotateY((float) Math.toRadians(mFrontLegsRot.y));
					MODELVIEW.scale(.2f, .05f, .05f);
					MODELVIEW.translate(.9f, 0, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Right leg
					MODELVIEW.push();
					MODELVIEW.translate(0, -.2f, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(-mFrontLegsRot.z));
					MODELVIEW.rotateY((float) Math.toRadians(mFrontLegsRot.y));
					MODELVIEW.scale(.2f, .05f, .05f);
					MODELVIEW.translate(.9f, 0, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
				}
				MODELVIEW.pop();
				//2 back legs
				MODELVIEW.push();
				MODELVIEW.translate(-.4f, 0, 0);
				{
					//Left leg
					MODELVIEW.push();
					MODELVIEW.translate(0, .3f, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mBackLegsRot.z));
					MODELVIEW.rotateY((float) Math.toRadians(mBackLegsRot.y));
					MODELVIEW.scale(.5f, .05f, .05f);
					MODELVIEW.translate(-.9f, 0, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Right leg
					MODELVIEW.push();
					MODELVIEW.translate(0, -.3f, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mBackLegsRot.z));
					MODELVIEW.rotateY((float) Math.toRadians(mBackLegsRot.y));
					MODELVIEW.scale(.5f, .05f, .05f);
					MODELVIEW.translate(-.9f, 0, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
				}
				MODELVIEW.pop();
				//4 left middle legs
				MODELVIEW.push();
				MODELVIEW.translate(0, 0.3f, 0);
				{	
					//Leg 1
					MODELVIEW.push();
					MODELVIEW.translate(.4f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mLeftLegsRot.z - 60));
					MODELVIEW.rotateX((float) Math.toRadians(mLeftLegsRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, 0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Leg 2
					MODELVIEW.push();
					MODELVIEW.translate(.2f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mLeftLegsRot.z - 30));
					MODELVIEW.rotateX((float) Math.toRadians(mLeftLegsRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, 0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Leg 3
					MODELVIEW.push();
					MODELVIEW.translate(0, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mLeftLegsRot.z));
					MODELVIEW.rotateX((float) Math.toRadians(mLeftLegsRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, 0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Leg 4
					MODELVIEW.push();
					MODELVIEW.translate(-.2f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mLeftLegsRot.z + 30));
					MODELVIEW.rotateX((float) Math.toRadians(mLeftLegsRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, 0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
				}
				MODELVIEW.pop();
				//4 right middle legs
				MODELVIEW.push();
				MODELVIEW.translate(0, -0.3f, 0);
				{
					//Leg 1
					MODELVIEW.push();
					MODELVIEW.translate(.4f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mRightLegsRot.z + 60));
					MODELVIEW.rotateX((float) Math.toRadians(mRightLegsRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, -0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Leg 2
					MODELVIEW.push();
					MODELVIEW.translate(.2f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mRightLegsRot.z + 30));
					MODELVIEW.rotateX((float) Math.toRadians(mRightLegsRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, -0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Leg 3
					MODELVIEW.push();
					MODELVIEW.translate(0, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mRightLegsRot.z));
					MODELVIEW.rotateX((float) Math.toRadians(mRightLegsRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, -0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
					//Leg 4
					MODELVIEW.push();
					MODELVIEW.translate(-.2f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mRightLegsRot.z - 30));
					MODELVIEW.rotateX((float) Math.toRadians(mRightLegsRot.x));
					MODELVIEW.scale(.05f, .5f, .05f);
					MODELVIEW.translate(0, -0.9f, 0);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
				}
				MODELVIEW.pop();
			}
			{ //Abdomen (1 hemisphere)
				renderer.setColorUniform(new Vector4f(0.8f, 0f, 0f, 1f));
				MODELVIEW.push();
				MODELVIEW.translate(-.9f, 0, .01f);
				MODELVIEW.scale(0.5f, 0.5f, 0.5f);
				renderer.setMatrixUniforms();
				Resources.mMeshList[Resources.MESH_ID_HEMISPHERE].draw();
				{ //Tail (1 box)
					renderer.setColorUniform(new Vector4f(0.8f, 0f, 0f, 1f));
					MODELVIEW.push();
					MODELVIEW.translate(-1f, 0, 0);
					MODELVIEW.rotateZ((float) Math.toRadians(mTailRot.z));
					MODELVIEW.rotateY((float) Math.toRadians(mTailRot.y));
					MODELVIEW.scale(1, 0.1f, 0.1f);
					MODELVIEW.translate(-.9f, 0, 1);
					renderer.setMatrixUniforms();
					Resources.mMeshList[Resources.MESH_ID_BOX].draw();
					MODELVIEW.pop();
				}
				MODELVIEW.pop();
			}
			MODELVIEW.pop();
		}
	}
	
	/**
	 * Run Ernest one step
	 */
	protected String stepErnest(boolean status) {
		// Sense the environment
//		int [][] matrix = new int [2][1];
//		matrix[0][0] = visualLeft();
//		matrix[1][0] = visualRight();
		
		mVisualMatrix[0][0] = visualLeft();
		mVisualMatrix[1][0] = visualRight();
		
		mSensorymotorSystem.senseMatrix(mVisualMatrix);
		String intention = mErnest.step(status);
		
		return intention;
	}
	/**
	 * Scan the left eye field.
	 * The start angle is radiant, counterclockwise on the screen
	 * @return Minimum distance to dirt if any, 1000 if no dirt detected. 
	 */
	protected int visualLeft() {
		float localStartAngle = 0;
		int distance = scanArc(localStartAngle , mEyeAngle, 20);
		mPixelLeft = SecaColor.getColor(mEyePixel); //Added to merge 'visualLeft' and 'pixelLeft' (from vacuum rev95)
		return distance;
	}
	
	/**
	 * Scan the right eye field.
	 * The start angle is radiant, counterclockwise on the screen
	 * @return Minimum distance to dirt if any, 1000 if no dirt detected. 
	 */
	protected int visualRight() {
		float localStartAngle = 0;
		int distance = scanArc(localStartAngle, -mEyeAngle, 20);
		mPixelRight = SecaColor.getColor(mEyePixel); //Added to merge 'visualRight' and 'pixelRight' (from vacuum rev95)
		return distance;
	}
	/** The hue color of a standard wall (green) */
	private static final float HUE_STANDARD_WALL      = 1/3f;
	/**
	 * Scan an arc from a viewpoint
	 * (The scan process is counterclockwise on the screen where the Y axis is downward).
	 * @param t The initial angular position
	 * @param a The arc's angular span
	 * @param d The arc's diameter (the agent's visual range)
	 * @return Minimum distance to dirt if any, 1000 if no dirt detected.
	 */
	protected int scanArc(float t, float a, float d) {
		if (d==0 || d>=Ernest.INFINITE)
			return Ernest.INFINITE;
		//Added to merge 'scanArc' and 'scanColor' (from vacuum rev95)
		float hue = HUE_STANDARD_WALL;
		float saturation = 1f;
		float luminance = .5f;
		//---
		int distance = Ernest.INFINITE;
		int i = Math.round(mPosition.getX());
		int j = Math.round(mPosition.getY());
		float startRad = (float) Math.toRadians(t + mRotation.getZ());
		float spanRad = (float) Math.toRadians(a);
		// The angle step corresponds to the size of a square on the arc 
		float step = (float) (Math.signum(a)*Math.asin(1/d));
		
		for (float rad = startRad;  (rad >= startRad - Math.abs(spanRad)) && (rad <= startRad + Math.abs(spanRad)); rad += step) {
			float x0 = (float) (mPosition.getX() + d * Math.cos(rad));
			float y0 = (float) (mPosition.getY() + d * Math.sin(rad)); // Y axis is upwards.
			Pair<Integer, Float> sense = null;
			if (mDiscrete) {
				int i0 = Math.round(x0);
				int j0 = Math.round(y0);
				sense = rayTrace(i, j , i0, j0);
			} else {
				sense = rayTrace(mPosition.getX(), mPosition.getY(), x0, y0);
			}
			int senseL = sense.mLeft;
			if (senseL < distance) distance = senseL;
			//Added to merge 'scanArc' and 'scanColor' (from vacuum rev95)
			float senseR = sense.mRight;
			if (senseR != HUE_STANDARD_WALL ) {
				// A singularity was found
				hue = senseR;
				luminance = 0.9f;
//				if (senseR > .6f)
				if (senseL != Ernest.INFINITE)
					// The singularity is a target
					saturation = 0.5f;
				break;
			}
			//---
		}
		mEyePixel = SecaColor.getHSBColor(hue, saturation, luminance); //Added to merge 'scanArc' and 'scanColor' (from vacuum rev95)
		return distance;
	}
	/**
	 * Scan the squares that are on a ray from a viewpoint to a target square
	 *  http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html 
	 * @return Distance to the dirty square if any, 1000 if no dirt. 
	 */
	protected Pair<Integer, Float> rayTrace(int i0, int j0, int i1, int j1) {
	    int di = Math.abs(i1 - i0);
	    int dj = Math.abs(j1 - j0);
	    int i = i0;
	    int j = j0;
	    int n = 1 + di + dj;
	    int i_inc = (i1 > i0) ? 1 : -1;
	    int j_inc = (j1 > j0) ? 1 : -1;
	    int error = di - dj;
	    di *= 2;
	    dj *= 2;
	    int k = Math.round(mPosition.getZ());
	    
	    for (; n > 0; --n) {
//	    	if ((i < 0) || (j < 0) || (i >= m_w) || (j >= m_h)) return Ernest.INFINITE;
	    	
	    	//Examine the square on the ray 
	    	System.out.println("i0="+i0+" j0="+j0+" i1="+i1+" j1="+j1+", i="+i+" j="+j+" k="+k);
	    	if (isWall(i, j, k)) {
	    		if (isStandardWall(i, j, k))
	    			return Pair.create(Ernest.INFINITE, HUE_STANDARD_WALL);
	    		else {
	    			int blockID = mSceneMgr.getBlockID(i, j, k, mName);
	    			return Pair.create(Ernest.INFINITE, SecaColor.getHueComponent(Block.blockList[blockID].getColor()));
	    		}
	    	}
	    	if (affordSee(i, j, k))
	    		return Pair.create(Math.abs(i - i0) + Math.abs(j - j0), SecaColor.getHueComponent(Block.blockList[mSceneMgr.getBlockID(i, j, k, mName)].getColor()));

	    	// move on along the ray
	    	if (error > 0) {
	    		i += i_inc;
	    		error -= dj;
	    	} else if (error < 0) {
	    		j += j_inc;
	    		error += di;
	    	} else {
	    		i += i_inc;
	    		j += j_inc;
	    		--n;
	    	}
	    }
	    return Pair.create(Ernest.INFINITE, HUE_STANDARD_WALL);
	}
	/**
	 * Scan the squares that are on a ray from a viewpoint to a target square
	 *  http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html 
	 * @return Distance to the dirty square if any, 1000 if no dirt. 
	 */
	protected Pair<Integer, Float> rayTrace(float x0, float y0, float x1, float y1) {
		float dx = Math.abs(x1 - x0);
		float dy = Math.abs(y1 - y0);
	    int i = (int) Math.round(x0);
	    int j = (int) Math.round(y0);
	    int n = 1;
	    int i_inc, j_inc;
	    float error;
	    int k = Math.round(mPosition.getZ());
	    float cornerTresh = .05f * dx * dy;

	    if (dx == 0) {
	        i_inc = 0;
	        error = Float.POSITIVE_INFINITY;
	    } else if (x1 > x0) {
	        i_inc = 1;
	        n += (int) Math.round(x1) - i;
	        error = (float) (((Math.round(x0) + .5f) - x0) * dy);
	    } else {
	        i_inc = -1;
	        n += i - (int) Math.round(x1);
	        error = (float) ((x0 - (Math.round(x0) - .5f)) * dy);
	    }
	    if (dy == 0) {
	        j_inc = 0;
	        error -= Float.POSITIVE_INFINITY;
	    } else if (y1 > y0) {
	        j_inc = 1;
	        n += (int) Math.round(y1) - j;
	        error -= ((Math.round(y0) + .5f) - y0) * dx;
	    } else {
	        j_inc = -1;
	        n += j - (int) Math.round(y1);
	        error -= (y0 - (Math.round(y0) - .5f)) * dx;
	    }
	    for (; n > 0; --n) {
	        //Examine the square on the ray 
	    	System.out.println("x0="+x0+" y0="+y0+" x1="+x1+" y1="+y1+", i="+i+" j="+j+" k="+k);
	    	if (isWall(i, j, k)) {
	    		if (isStandardWall(i, j, k))
	    			return Pair.create(Ernest.INFINITE, HUE_STANDARD_WALL);
	    		else {
	    			int blockID = mSceneMgr.getBlockID(i, j, k,mName);
	    			return Pair.create(Ernest.INFINITE, SecaColor.getHueComponent(Block.blockList[blockID].getColor()));
	    		}
	    	}
	    	if (affordSee(i, j, k))
	    		return Pair.create(Math.round(Math.abs(i - x0) + Math.abs(j - y0)), SecaColor.getHueComponent(Block.blockList[mSceneMgr.getBlockID(i, j, k, mName)].getColor()));

	        // move on along the ray
	        if (error > cornerTresh) {
	            j += j_inc;
	            error -= dx;
	        } else if (error < -cornerTresh) {
	            i += i_inc;
	            error += dy;
	        } else {
	        	i += i_inc;
	    		j += j_inc;
	    		error += dy - dx;
	    		--n;
	        }
	    }
		
		return Pair.create(Ernest.INFINITE, HUE_STANDARD_WALL);
	}
	
	/**
	 * Enact the primitive schema chosen by Ernest.
	 * @return binary feedback. 
	 */
	protected boolean enactSchema(String schema) {
		
//		//TODO Add the tracer
//		System.out.println("Step #" + m_counter + "=======");
//		m_tracer.writeLine(" " + m_counter++ );
		
		// Sucking the squares is automatic
		suck();
		
		boolean status = false;
		
	    if (schema.equals("v"))
			status = turnRight();
		else if (schema.equals("^"))
			status = turnLeft();
		else if (schema.equals(">"))
			status = stepForward();
	    return status;
	}
	
	/**
	 * Turn.
	 * @param angle in degrees. A positive angle will make the agent turn on the left.
	 * @return true if adjacent wall, false if adjacent empty. 
	 */
	protected boolean turn(float angle) {
		mRotation.z += angle;
		return !isWall(localToParentRef(DIRECTION_AHEAD));
	}
	protected boolean turnLeft() {
		if (mAnimation != null)
			mAnimation.init(Ernest8Anim.ANIM_TURN_LEFT);
		if (mDiscrete)
			return turn(DISCRITE_ROTATION_STEP);
		else
			return turn(CONTINUOUS_ROTATION_STEP);
	}
	protected boolean turnRight() {
		if (mAnimation != null)
			mAnimation.init(Ernest8Anim.ANIM_TURN_RIGHT);
		if (mDiscrete)
			return turn(-DISCRITE_ROTATION_STEP);
		else
			return turn(-CONTINUOUS_ROTATION_STEP);
	}
	
	/**
	 * Move forward.
	 * @return true if adjacent wall, false if adjacent empty. 
	 */
	protected boolean goForward(float distance) {
//		Vector3f trans = new Vector3f(DIRECTION_AHEAD);
//		trans.scale(distance);
//		Vector3f nextLocalFrontPos = new Vector3f(trans);
//		//TODO Add intersection test to detect collision
//		//nextLocalFrontPos.x += getLocalAABB().getHalfSize().x;
//		if (isWall(localToParentRef(nextLocalFrontPos))) {
//			//The adjacent position is a wall
//			if (mAnimation != null)
//				mAnimation.init(Ernest8Anim.ANIM_BUMP);
//			return false;
//		} else {
//			//The adjacent position is free
//			if (mAnimation != null)
//				mAnimation.init(Ernest8Anim.ANIM_MOVE);
//			mPosition.set(localToParentRef(trans));
//			if (mDiscrete)
//				roundPosition();
//			return true;
//		}
		
		Vector3f localTarget = new Vector3f(DIRECTION_AHEAD);
		localTarget.scale(distance);
		if (mDiscrete) {
			if (isWall(localToParentRef(localTarget))) {
				//The adjacent position is a wall
				if (mAnimation != null)
					mAnimation.init(Ernest8Anim.ANIM_BUMP);
				return false;
			} else {
				//The adjacent position is free
				if (mAnimation != null)
					mAnimation.init(Ernest8Anim.ANIM_MOVE);
				mPosition.set(localToParentRef(localTarget));
				roundPosition();
				return true;
			}
		} else {
			Vector3f localTrans = getNonCollidingLocalTrans(localTarget);
			if (isBumping(localTarget, localTrans)) {
				//The adjacent position is a wall
				if (mAnimation != null)
					mAnimation.init(Ernest8Anim.ANIM_BUMP);
				mPosition.set(localToParentRef(localTrans));
				return false;
			} else {
				//The adjacent position is free
				if (mAnimation != null)
					mAnimation.init(Ernest8Anim.ANIM_MOVE);
				mPosition.set(localToParentRef(localTrans));
				return true;
			}
		}
	}
	protected boolean stepForward() {
		if (mDiscrete) {
			return goForward(DISCRITE_TRANSLATION_STEP);
		} else
			return goForward(CONTINUOUS_TRANSLATION_STEP);
	}
	protected Vector3f getNonCollidingLocalTrans(Vector3f localTarget) {
		Vector3f localTrans = new Vector3f();
		float s;
		for(s=.1f; s<=1; s+=.1f) {
			localTrans.scale(s, localTarget);
			if (isColliding(localTrans))
				break;
		}
		s -= .1f;
		localTrans.scale(s, localTarget);
		return localTrans;
	}
	protected boolean isColliding(Vector3f localTrans) {
		Vector3f point = new Vector3f();
		point.scaleAdd(mLocalBoundingSphere.getRadius(), DIRECTION_AHEAD, localTrans);
		if (isWall(localToParentRef(point)))
			return true;
		point.scaleAdd(mLocalBoundingSphere.getRadius(), DIRECTION_AHEAD_LEFT, localTrans);
		if (isWall(localToParentRef(point)))
			return true;
		point.scaleAdd(mLocalBoundingSphere.getRadius(), DIRECTION_AHEAD_RIGHT, localTrans);
		if (isWall(localToParentRef(point)))
			return true;
		point.scaleAdd(mLocalBoundingSphere.getRadius(), DIRECTION_LEFT, localTrans);
		if (isWall(localToParentRef(point)))
			return true;
		point.scaleAdd(mLocalBoundingSphere.getRadius(), DIRECTION_RIGHT, localTrans);
		if (isWall(localToParentRef(point)))
			return true;
		return false;
	}
	protected boolean isBumping(Vector3f localTarget, Vector3f localTrans) {
		Vector3f diff = new Vector3f();
		diff.sub(localTarget, localTrans);
		return (diff.lengthSquared() > 0.01);
	}
	protected void roundPosition() {
		mPosition.setX(Math.round(mPosition.getX()));
		mPosition.setY(Math.round(mPosition.getY()));
		mPosition.setZ(Math.round(mPosition.getZ()));
	}
	
//	/**
//	 * Touch the square forward.
//	 * @return true if wall, false if empty. 
//	 */
//	protected boolean touch(Vector3f vec) {
//		Vector3f scenePos = localToParentRef(vec);
//		return isWall(scenePos);
//	}
//	protected boolean touchForward() {
//		if (mAnimation != null)
//			mAnimation.init(Ernest8Anim.ANIM_TOUCH_FORWARD);
//		return touch(DIRECTION_AHEAD);
//	}
//	protected boolean touchLeft() {
//		if (mAnimation != null)
//			mAnimation.init(Ernest8Anim.ANIM_TOUCH_LEFT);
//		return touch(DIRECTION_LEFT);
//	}
//	protected boolean touchRight() {
//		if (mAnimation != null)
//			mAnimation.init(Ernest8Anim.ANIM_TOUCH_RIGHT);
//		return touch(DIRECTION_RIGHT);
//	}
	
	//TODO Add materials
	protected boolean isWall(int i, int j, int k) {
		int blockID = mSceneMgr.getBlockID(i, j, k, mName);
		if (blockID == -1)
			return true;
		return !Block.blockList[blockID].getMaterial().affordWalk();
	}
	protected boolean isWall(Vector3f pos) {
		return isWall(Math.round(pos.x), Math.round(pos.y), Math.round(pos.z));
	}
	protected boolean affordSee(int i, int j, int k) {
		int blockID = mSceneMgr.getBlockID(i, j, k, mName);
		if (blockID == -1)
			return false;
		return Block.blockList[blockID].getMaterial().affordSee();
	}
	protected boolean affordSee(Vector3f pos) {
		return affordSee(Math.round(pos.x), Math.round(pos.y), Math.round(pos.z));
	}
	protected boolean affordEat(int i, int j, int k) {
		int blockID = mSceneMgr.getBlockID(i, j, k, mName);
		if (blockID == -1)
			return false;
		return Block.blockList[blockID].getMaterial().affordEat();
	}
	protected boolean affordEat(Vector3f pos) {
		return affordEat(Math.round(pos.x), Math.round(pos.y), Math.round(pos.z));
	}
	
	protected boolean isStandardWall(int i, int j, int k) {
		int blockID = mSceneMgr.getBlockID(i, j, k, mName);
		if (blockID == -1 || blockID == Block.stone.getBlockID() || blockID == Block.stoneSlab.getBlockID() )
			return true;
		return !Block.blockList[blockID].getMaterial().affordWalk();
	}
	protected boolean isStandartWall(Vector3f pos) {
		return isStandardWall(Math.round(pos.x), Math.round(pos.y), Math.round(pos.z));
	}
	
	protected void suck() {
		if (affordEat(mPosition))
			mSceneMgr.setBlockID(mPosition, 0);
	}
	
	private final int DISCRITE_ROTATION_STEP = 90;
	private final float DISCRITE_TRANSLATION_STEP = 1;
	private final int CONTINUOUS_ROTATION_STEP = 45;
	private final float CONTINUOUS_TRANSLATION_STEP = 1f;
	//Agent related
	private Ernest mErnest;
	private Visual82SensorymotorSystem mSensorymotorSystem;
//	protected ITracer mTracer;
	private String mIntention;
	private boolean mStatus;
	/** The angular field of each eye. */
	private float mEyeAngle;
	private Color mEyePixel;
	private Color mPixelLeft;
	private Color mPixelRight;
	private int[][] mVisualMatrix;
	
	/**
	 * Value of the diagonal projection in 2D:
	 * 1 for a square diagonal,
	 * 1/sqrt(2) for a circle diagonal.
	 */
	final private float DIAG2D_PROJ = SecaMath.INV_SQRT_2;
	/**
	 * Value of the diagonal projection in 3D:
	 * 1 for a cube diagonal,
	 * 1/sqrt(3) for a sphere diagonal.
	 */
	final private float DIAG3D_PROJ = SecaMath.INV_SQRT_3;
	//Local directions
	final private Vector3f DIRECTION_AHEAD = new Vector3f(1, 0, 0);
	final private Vector3f DIRECTION_BEHIND = new Vector3f(-1, 0, 0);
	final private Vector3f DIRECTION_LEFT = new Vector3f(0, 1, 0);
	final private Vector3f DIRECTION_RIGHT = new Vector3f(0, -1, 0);
	final private Vector3f DIRECTION_AHEAD_LEFT = new Vector3f(DIAG2D_PROJ, DIAG2D_PROJ, 0);
	final private Vector3f DIRECTION_AHEAD_RIGHT = new Vector3f(DIAG2D_PROJ, -DIAG2D_PROJ, 0);
	final private Vector3f DIRECTION_BEHIND_LEFT = new Vector3f(-DIAG2D_PROJ, DIAG2D_PROJ, 0);
	final private Vector3f DIRECTION_BEHIND_RIGHT = new Vector3f(-DIAG2D_PROJ, -DIAG2D_PROJ, 0);
	
}
